#include "esp_camera.h"
#include "FS.h"
#include "SD_MMC.h"
#include "soc/soc.h"
#include "soc/rtc_cntl_reg.h"
#include "esp32-hal-cpu.h"

// ==========================================
//  PINS & SETTINGS
// ==========================================
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27
#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22

#define BUTTON_PIN        13 
#define FLASH_PIN          4 

// RESOLUTION: HVGA (480x320)
#define CAPTURE_WIDTH     480
#define CAPTURE_HEIGHT    320

const int flashFreq = 5000;
const int flashResolution = 8;
const int flashDim = 12; 
const int saveDim = 2;   

// TIMERS
#define SAVE_TIMEOUT  10000 
#define SLEEP_TIMEOUT 30000 

unsigned long lastActionTime = 0;

// ==========================================
//  BUFFER (RAM)
// ==========================================
#define MAX_BUFFERED_PHOTOS 15

struct PhotoBuffer {
  uint8_t* data;
  size_t len;
};

PhotoBuffer ramBuffer[MAX_BUFFERED_PHOTOS];
int bufferIndex = 0;

// ==========================================
//  PALETTE
// ==========================================
const uint8_t BMP_PALETTE[8][4] = {
  {0x12, 0x0E, 0x0E, 0}, {0x24, 0x1A, 0x1A, 0}, {0x46, 0x33, 0x33, 0}, {0x73, 0x53, 0x53, 0}, 
  {0xA4, 0x80, 0x80, 0}, {0xBF, 0xA6, 0xA6, 0}, {0xD2, 0xC1, 0xC1, 0}, {0xEC, 0xE6, 0xE6, 0}  
};
const uint8_t GRAY_LEVELS[8] = {15, 40, 70, 100, 135, 170, 205, 240};
uint8_t COLOR_LUT[256]; 
int photoCount = 1;

// Function Declarations
void generateLUT();
int getLastPhotoNumber();
void saveAllBufferedPhotos();
void ditherOptimized(uint8_t *img, int w, int h);
void flipBufferVertically(uint8_t* data, int w, int h);
void signalSDErrorAndSleep(); 
void goToDeepSleep();
void blinkLED(int count, int speed);

void setup() {
  setCpuFrequencyMhz(240); 
  
  Serial.begin(115200);
  Serial.println("\n-----------------------------------");
  Serial.println("[SYSTEM] Starting GOTHIC CAM (MANUAL SLEEP)...");
  
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); 
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  
  generateLUT();

  ledcAttach(FLASH_PIN, flashFreq, flashResolution);
  ledcWrite(FLASH_PIN, 0); 

  // SD CHECK -> STROBE
  if (!SD_MMC.begin("/sdcard", true, false, SDMMC_FREQ_HIGHSPEED)) {
     if (!SD_MMC.begin("/sdcard", true)) {
        Serial.println("[ERROR] No SD! STROBE!");
        signalSDErrorAndSleep(); 
        return; 
     }
  }
  
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sscb_sda = SIOD_GPIO_NUM;
  config.pin_sscb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  config.xclk_freq_hz = 20000000;
  
  config.pixel_format = PIXFORMAT_GRAYSCALE; 
  config.frame_size = FRAMESIZE_HVGA; 
  config.jpeg_quality = 10; 
  config.fb_count = 2; 

  if (esp_camera_init(&config) != ESP_OK) {
      signalSDErrorAndSleep(); 
      return;
  }
  
  // ==========================================================
  // ULTRA BRIGHT & FAST SETTINGS
  // ==========================================================
  sensor_t * s = esp_camera_sensor_get();
  s->set_exposure_ctrl(s, 1);
  s->set_aec2(s, 1); 
  
  // 1. FAST SHUTTER (-2)
  s->set_ae_level(s, -2);      

  // 2. DIGITAL BOOST (MAX ISO + Gamma)
  s->set_gain_ctrl(s, 1);
  s->set_gainceiling(s, (gainceiling_t)6); 
  s->set_brightness(s, 2);   
  s->set_contrast(s, 1);     
  
  // Advanced Corrections
  s->set_lenc(s, 1);    // Lens Correction (კუთხეების განათება)
  s->set_raw_gma(s, 1); // Gamma Correction (ჩრდილების ამოწევა)
  s->set_dcw(s, 1);     // Denoise (ხმაურის შემცირება)

  s->set_whitebal(s, 1); 
  s->set_awb_gain(s, 1); 
  s->set_wb_mode(s, 0);

  photoCount = getLastPhotoNumber() + 1;
  lastActionTime = millis();
  
  Serial.printf("[READY] Manual Sleep Enabled (Hold 3s).\n");
}

void loop() {
  unsigned long now = millis();

  // 1. AUTO-SAVE
  if (bufferIndex > 0 && (now - lastActionTime > SAVE_TIMEOUT)) {
      saveAllBufferedPhotos();
      lastActionTime = millis(); 
  }

  // 2. AUTO-SLEEP
  if (bufferIndex == 0 && (now - lastActionTime > SLEEP_TIMEOUT)) {
      goToDeepSleep();
  }

  // 3. BUTTON LOGIC (NEW: LONG PRESS)
  if (digitalRead(BUTTON_PIN) == LOW) {
    lastActionTime = millis();
    unsigned long pressStart = millis();
    bool longPressDetected = false;

    // ველოდებით სანამ ღილაკი დაჭერილია
    while (digitalRead(BUTTON_PIN) == LOW) {
        // თუ 3 წამზე მეტია დაჭერილი
        if (millis() - pressStart > 3000) {
            longPressDetected = true;
            Serial.println(">>> FORCED SLEEP (Long Press) <<<");
            
            // ვიზუალური ნიშანი (3 ნელი ციმციმი)
            blinkLED(3, 300); 
            goToDeepSleep(); // აქ კამერა ითიშება
        }
        delay(10);
    }

    // თუ ღილაკს აუშვი და ეს არ იყო "Long Press", ესეიგი ფოტოს ვიღებთ
    if (!longPressDetected) {
        
        // --- TAKE PHOTO LOGIC START ---
        
        if (bufferIndex >= MAX_BUFFERED_PHOTOS) {
            saveAllBufferedPhotos();
            lastActionTime = millis();
        }

        size_t freeMem = heap_caps_get_free_size(MALLOC_CAP_SPIRAM);
        if(freeMem < 100000) { 
            saveAllBufferedPhotos();
        }

        Serial.printf("\n>>> SNAP (%d/%d) <<<\n", bufferIndex + 1, MAX_BUFFERED_PHOTOS);
        ledcWrite(FLASH_PIN, flashDim);
        
        // WARM-UP
        for(int i=0; i<2; i++){
            camera_fb_t *dummy = esp_camera_fb_get();
            if(dummy) esp_camera_fb_return(dummy);
        }

        camera_fb_t *fb = esp_camera_fb_get();
        if (!fb) { ledcWrite(FLASH_PIN, 0); return; }

        uint8_t* workBuffer = (uint8_t*)heap_caps_malloc(fb->len, MALLOC_CAP_SPIRAM);
        if(workBuffer) {
            memcpy(workBuffer, fb->buf, fb->len);
            esp_camera_fb_return(fb); 

            ditherOptimized(workBuffer, CAPTURE_WIDTH, CAPTURE_HEIGHT);
            flipBufferVertically(workBuffer, CAPTURE_WIDTH, CAPTURE_HEIGHT);
            
            ramBuffer[bufferIndex].data = workBuffer;
            ramBuffer[bufferIndex].len = CAPTURE_WIDTH * CAPTURE_HEIGHT;
            bufferIndex++;

            if (bufferIndex >= MAX_BUFFERED_PHOTOS) {
                ledcWrite(FLASH_PIN, 0); 
                saveAllBufferedPhotos(); 
                lastActionTime = millis(); 
            }
        } else {
            Serial.println("[ERROR] RAM Full!");
            esp_camera_fb_return(fb);
        }
        ledcWrite(FLASH_PIN, 0);
        delay(50); 
        // --- TAKE PHOTO LOGIC END ---
    }
  }
}

void saveAllBufferedPhotos() {
    Serial.println(">>> SAVING... <<<");
    for(int i=0; i<bufferIndex; i++) {
        ledcWrite(FLASH_PIN, saveDim); 
        
        char filename[32];
        sprintf(filename, "/PIC_%03d.bmp", photoCount++);
        
        File f = SD_MMC.open(filename, FILE_WRITE);
        if(f) {
            int w = CAPTURE_WIDTH; int h = CAPTURE_HEIGHT;
            uint32_t imageSize = w * h;
            uint32_t fileSize = 54 + 1024 + imageSize;
            uint32_t offset = 54 + 1024;
            
            uint8_t bmpHeader[54] = { 'B', 'M', (uint8_t)(fileSize), (uint8_t)(fileSize >> 8), (uint8_t)(fileSize >> 16), (uint8_t)(fileSize >> 24), 0, 0, 0, 0, (uint8_t)(offset), (uint8_t)(offset >> 8), (uint8_t)(offset >> 16), (uint8_t)(offset >> 24), 40, 0, 0, 0, (uint8_t)(w), (uint8_t)(w >> 8), (uint8_t)(w >> 16), (uint8_t)(w >> 24), (uint8_t)(h), (uint8_t)(h >> 8), (uint8_t)(h >> 16), (uint8_t)(h >> 24), 1, 0, 8, 0, 0, 0, 0, 0, (uint8_t)(imageSize), (uint8_t)(imageSize >> 8), (uint8_t)(imageSize >> 16), (uint8_t)(imageSize >> 24), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
            f.write(bmpHeader, 54);
            
            uint8_t paletteData[1024] = {0};
            for(int k=0; k<8; k++){ paletteData[k*4]=BMP_PALETTE[k][0]; paletteData[k*4+1]=BMP_PALETTE[k][1]; paletteData[k*4+2]=BMP_PALETTE[k][2]; paletteData[k*4+3]=0; }
            f.write(paletteData, 1024);
            
            f.write(ramBuffer[i].data, ramBuffer[i].len);
            f.close();
            Serial.printf("[SAVED] %s\n", filename);
        }
        
        if(ramBuffer[i].data) {
            free(ramBuffer[i].data);
            ramBuffer[i].data = NULL;
        }
        
        ledcWrite(FLASH_PIN, 0);
        delay(50);
    }
    bufferIndex = 0;
    Serial.println(">>> DONE <<<");
    blinkLED(2, 100);
}

void signalSDErrorAndSleep() {
    Serial.println("!!! STROBE ALARM !!!");
    for(int k=0; k<2; k++) {
        for(int i=0; i<10; i++) { 
            ledcWrite(FLASH_PIN, flashDim);
            delay(100); 
            ledcWrite(FLASH_PIN, 0);
            delay(100); 
        }
        delay(500);
    }
    goToDeepSleep();
}

void generateLUT() {
    for (int i = 0; i < 256; i++) {
        uint8_t best = 0; int minD = 9999;
        for (int k = 0; k < 8; k++) { int d = abs(i - GRAY_LEVELS[k]); if(d < minD){ minD = d; best = k; } }
        COLOR_LUT[i] = best;
    }
}

void ditherOptimized(uint8_t *img, int w, int h) {
  int16_t *errors = (int16_t*)heap_caps_calloc(w * h, sizeof(int16_t), MALLOC_CAP_SPIRAM);
  if(!errors) return;
  for (int y = 0; y < h; y++) {
    for (int x = 0; x < w; x++) {
      int idx = y * w + x;
      int16_t oldVal = img[idx] + errors[idx];
      int clampedVal = (oldVal < 0) ? 0 : (oldVal > 255 ? 255 : oldVal);
      uint8_t best = COLOR_LUT[clampedVal];
      img[idx] = best; 
      int16_t interr = oldVal - GRAY_LEVELS[best];
      if (x+1<w) errors[idx+1]+=interr*7/16;
      if (y+1<h) { if(x>0)errors[idx+w-1]+=interr*3/16; errors[idx+w]+=interr*5/16; if(x+1<w)errors[idx+w+1]+=interr/16; }
    }
  }
  free(errors);
}

void flipBufferVertically(uint8_t* data, int w, int h) {
    uint8_t* tempRow = (uint8_t*)heap_caps_malloc(w, MALLOC_CAP_SPIRAM);
    if(!tempRow) return;
    for(int y = 0; y < h / 2; y++) {
        uint8_t* topRow = data + (y * w);
        uint8_t* botRow = data + ((h - 1 - y) * w);
        memcpy(tempRow, topRow, w); memcpy(topRow, botRow, w); memcpy(botRow, tempRow, w);
    }
    free(tempRow);
}

void goToDeepSleep() {
    ledcWrite(FLASH_PIN, 0);
    pinMode(PWDN_GPIO_NUM, OUTPUT);
    digitalWrite(PWDN_GPIO_NUM, HIGH);
    esp_sleep_enable_ext0_wakeup((gpio_num_t)BUTTON_PIN, 0);
    esp_deep_sleep_start();
}

void blinkLED(int count, int speed) {
    for(int i=0; i<count; i++){
        ledcWrite(FLASH_PIN, flashDim); delay(speed);
        ledcWrite(FLASH_PIN, 0); delay(speed);
    }
}

int getLastPhotoNumber() {
  int maxNum = 0;
  File root = SD_MMC.open("/");
  File file = root.openNextFile();
  while(file){
    String name = file.name();
    if(name.indexOf("PIC_") >= 0 && name.endsWith(".bmp")){
      int startIndex = name.indexOf("PIC_") + 4;
      int num = name.substring(startIndex, startIndex + 3).toInt();
      if(num > maxNum) maxNum = num;
    }
    file = root.openNextFile();
  }
  return maxNum;
}